#pragma once
#include "Quaternion.h"

namespace Utility{
	namespace Math{
		template <unsigned int Rows = 4, unsigned int Collumns = 4,typename InternalType = float>
		class Matrix{
		public:
			Matrix(){
				identity();
			}

			Matrix(InternalType *data){
				memcpy(m_data,data,sizeof(InternalType)*Rows*Collumns);
			}

			~Matrix(){
			}

			void createFromAxisAngle(Vector<float> axis, float angle){
				if(Rows < 3 || Collumns < 3){
					LOG_ERROR("At least an 3x3 matrix needed for this function to work.");
					assert(0);
					return;
				}

				float c = cosf(angle);
			    float s = sinf(angle);
			    float t = 1.0f - c;
				axis.normalize();

			    set(0,0,c + axis(_x_)*axis(_x_)*t);
			    set(1,1,c + axis(_y_)*axis(_y_)*t);
			    set(2,2,c + axis(_z_)*axis(_z_)*t);

			    float tmp1 = axis(_x_)*axis(_y_)*t;
			    float tmp2 = axis(_z_)*s;
			    set(0,1,tmp1 + tmp2);
			    set(1,0,tmp1 - tmp2);
			    tmp1 = axis(_x_)*axis(_z_)*t;
			    tmp2 = axis(_y_)*s;
			    set(0,2,tmp1 - tmp2);
			    set(2,0,tmp1 + tmp2);    
				tmp1 = axis(_y_)*axis(_z_)*t;
			    tmp2 = axis(_x_)*s;
			    set(1,2,tmp1 + tmp2);
			    set(2,1,tmp1 - tmp2);
			}

			void createFromQuaternion(Quaternion<InternalType> &quat){
				if(Rows < 3 || Collumns < 3){
					LOG_ERROR("At least an 3x3 matrix needed for this function to work.");
					assert(0);
					return;
				}

				quat.normalize();

				InternalType fTx  = 2.0f*quat.vec(_x_);
		        InternalType fTy  = 2.0f*quat.vec(_y_);
		        InternalType fTz  = 2.0f*quat.vec(_z_);
		        InternalType fTwx = fTx*quat.vec(_w_);
		        InternalType fTwy = fTy*quat.vec(_w_);
		        InternalType fTwz = fTz*quat.vec(_w_);
		        InternalType fTxx = fTx*quat.vec(_x_);
		        InternalType fTxy = fTy*quat.vec(_x_);
		        InternalType fTxz = fTz*quat.vec(_x_);
		        InternalType fTyy = fTy*quat.vec(_y_);
		        InternalType fTyz = fTz*quat.vec(_y_);
		        InternalType fTzz = fTz*quat.vec(_z_);

				identity();

		        set(0,0,1.0f-(fTyy+fTzz));
		        set(0,1,fTxy-fTwz);
		        set(0,2,fTxz+fTwy);
		        set(1,0,fTxy+fTwz);
		        set(1,1,1.0f-(fTxx+fTzz));
		        set(1,2,fTyz-fTwx);
		        set(2,0,fTxz-fTwy);
		        set(2,1,fTyz+fTwx);
		        set(2,2,1.0f-(fTxx+fTyy));
			}

			void identity(){
				unsigned int oneIndex = 0;
				for(unsigned int row=0; row<Rows; row++){
					for(unsigned int col=0; col<Collumns; col++){
						if(col == oneIndex)
							m_data[row*Collumns+col] = InternalType(1.0);
						else{
							m_data[row*Collumns+col] = InternalType(0.0);
						}
					}
					oneIndex++;
				}
			}

			void transpose(){
				Matrix<Rows,Collumns,InternalType> orig = *this;

				for(unsigned int row=0; row<Rows; row++){
					for(unsigned int col=0; col<Collumns; col++){
						set(col,row,orig.get(row,col));
					}
				}
			}

			InternalType get(unsigned int row, unsigned int col){
				if(row>Rows || col>Collumns){
					LOG_ERROR("Trying to get value outside matrix range.");
					assert(0);
					return InternalType();
				}

				return m_data[(Collumns*row)+col];
			}

			void set(unsigned int row, unsigned int col, InternalType value){
				m_data[(Collumns*row)+col] = value;
			}

			// Operator overloading
			// Multiplication
			Matrix<Rows,Collumns,InternalType> operator* (Matrix<Rows,Collumns,InternalType> &other){
				Matrix<Rows,Collumns,InternalType> result;

				for(unsigned int row=0; row<Rows; row++){
					for(unsigned int col=0; col<Collumns; col++){
						result.set(row,col,multi(row,col,other));
					}
				}

				return result;
			}

			void operator*= (Matrix<Rows,Collumns,InternalType> &other){
				Matrix<Rows,Collumns,InternalType> result;

				for(unsigned int row=0; row<Rows; row++){
					for(unsigned int col=0; col<Collumns; col++){
						result.set(row,col,multi(row,col,other));
					}
				}

				*this = result;
			}

			void transform(Vector<float> &vec){
				Vector<float> orig = vec;
				vec(_x_,multi(0,orig));
				vec(_y_,multi(1,orig));
				vec(_z_,multi(2,orig));
			}

			InternalType	m_data[Rows*Collumns];

		private:
			InternalType multi(int start1, int start2, Matrix<Rows,Collumns,InternalType> &other){
				InternalType result = 0;

				for(int i=0; i<Rows; i++){
					result += get(start1,i)*other.get(i,start2);
				}

				return result;
			}

			InternalType multi(int start1, Vector<float> &other){
				InternalType result = 0;

				for(int i=0; i<3; i++){
					result += get(i,start1)*other(VectorPos(i));
				}

				return result;
			}
		};
	}
}